#include "mpudmp.h"
#include "mpudmp_internal.h"

#define MPU9250_DMP_CODE_SIZE       1962    // dmpMemory[]
#define MPU9250_DMP_CONFIG_SIZE     232     // dmpConfig[]
#define MPU9250_DMP_UPDATES_SIZE    140     // dmpUpdates[]

/* ================================================================================================ *
 | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
 |                                                                                                  |
 | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
 |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
 |                                                                                                  |
 | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
 |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
 * ================================================================================================ */

// this block of memory gets written to the MPU on start-up, and it seems
// to be volatile memory, so it has to be done each time (it only takes ~1
// second though)
const unsigned char dmpMemory[MPU9250_DMP_CODE_SIZE] =
{
	// bank 0, 256 bytes
	0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
	0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
	0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
	0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
	0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
	0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
	0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,

	// bank 1, 256 bytes
	0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
	0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
	0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
	0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
	0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,

	// bank 2, 256 bytes
	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,

	// bank 3, 256 bytes
	0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
	0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
	0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
	0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
	0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
	0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
	0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
	0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
	0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
	0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
	0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
	0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
	0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
	0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
	0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
	0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,

	// bank 4, 256 bytes
	0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
	0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
	0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
	0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
	0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
	0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
	0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
	0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
	0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
	0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
	0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
	0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,

	// bank 5, 256 bytes
	0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
	0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
	0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
	0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
	0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
	0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
	0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
	0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
	0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
	0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
	0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
	0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
	0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
	0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
	0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
	0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,

	// bank 6, 256 bytes
	0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
	0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
	0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
	0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
	0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
	0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
	0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
	0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
	0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
	0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
	0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
	0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
	0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
	0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
	0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
	0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,

	// bank 7, 170 bytes (remainder)
	0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
	0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
	0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
	0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
	0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
	0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
	0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
	0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
	0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
	0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
	0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
};

const unsigned char dmpConfig[MPU9250_DMP_CONFIG_SIZE] PROGMEM =
{
//  BANK    OFFSET  LENGTH  [DATA]
	0x02,   0xEC,   0x04,   0x00, 0x47, 0x7D, 0x1A,   // ?
	0x03,   0x82,   0x03,   0x4C, 0xCD, 0x6C,         // FCFG_1 inv_set_gyro_calibration
	0x03,   0xB2,   0x03,   0x36, 0x56, 0x76,         // FCFG_3 inv_set_gyro_calibration
	0x00,   0x68,   0x04,   0x02, 0xCA, 0xE3, 0x09,   // D_0_104 inv_set_gyro_calibration
	0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // D_1_152 inv_set_accel_calibration
	0x03,   0x86,   0x03,   0x0C, 0xC9, 0x2C,         // FCFG_2 inv_set_accel_calibration
	0x03,   0x90,   0x03,   0x26, 0x46, 0x66,         //   (continued)...FCFG_2 inv_set_accel_calibration
	0x00,   0x6C,   0x02,   0x40, 0x00,               // D_0_108 inv_set_accel_calibration

	0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_00 inv_set_compass_calibration
	0x02,   0x44,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_01
	0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_02
	0x02,   0x4C,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_10
	0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_11
	0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_12
	0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_20
	0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_21
	0x02,   0xBC,   0x04,   0xC0, 0x00, 0x00, 0x00,   // CPASS_MTX_22

	0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,   // D_1_236 inv_apply_endian_accel
	0x03,   0x86,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
	0x04,   0x22,   0x03,   0x0D, 0x35, 0x5D,         // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
	0x00,   0xA3,   0x01,   0x00,                     // ?
	0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,   // FCFG_5 inv_set_bias_update
	0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
	0x07,   0x9F,   0x01,   0x30,                     // CFG_16 inv_set_footer
	0x07,   0x67,   0x01,   0x9A,                     // CFG_GYRO_SOURCE inv_send_gyro
	0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_9 inv_send_gyro -> inv_construct3_fifo
	0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
	0x02,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // ?
	0x07,   0x83,   0x06,   0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
	// SPECIAL 0x01 = enable interrupts
	0x00,   0x00,   0x00,   0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
	0x07,   0xA7,   0x01,   0xFE,                     // ?
	0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
	0x07,   0x67,   0x01,   0x9A,                     // ?
	0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
	0x07,   0x8D,   0x04,   0xF1, 0x28, 0x30, 0x38,   // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
	0x02,   0x16,   0x02,   0x00, 0x03                // D_0_22 inv_set_fifo_rate

	// This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
	// 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
	// DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

	// It is important to make sure the host processor can keep up with reading and processing
	// the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
};

const unsigned char dmpUpdates[MPU9250_DMP_UPDATES_SIZE] PROGMEM =
{
	0x01,   0xB2,   0x02,   0xFF, 0xF5,
	0x01,   0x90,   0x04,   0x0A, 0x0D, 0x97, 0xC0,
	0x00,   0xA3,   0x01,   0x00,
	0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,
	0x01,   0x6A,   0x02,   0x06, 0x00,
	0x01,   0x60,   0x08,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
	0x02,   0x60,   0x0C,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x01,   0x08,   0x02,   0x01, 0x20,
	0x01,   0x0A,   0x02,   0x00, 0x4E,
	0x01,   0x02,   0x02,   0xFE, 0xB3,
	0x02,   0x6C,   0x04,   0x00, 0x00, 0x00, 0x00, // READ
	0x02,   0x6C,   0x04,   0xFA, 0xFE, 0x00, 0x00,
	0x02,   0x60,   0x0C,   0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
	0x02,   0xF4,   0x04,   0x00, 0x00, 0x00, 0x00,
	0x02,   0xF8,   0x04,   0x00, 0x00, 0x00, 0x00,
	0x02,   0xFC,   0x04,   0x00, 0x00, 0x00, 0x00,
	0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
	0x00,   0x60,   0x04,   0x00, 0x40, 0x00, 0x00
};

uint8_t MPU9250::dmpInitialize()
{
	// reset device
	DEBUG_PRINTLN("Resetting MPU9250...");
	reset();
	mpuDelayMs(30); // wait after reset

	// enable sleep mode and wake cycle
	/*Serial.println("Enabling sleep mode...");
	setSleepEnabled(true);
	Serial.println("Enabling wake cycle...");
	setWakeCycleEnabled(true);*/

	// disable sleep mode
	DEBUG_PRINTLN("Disabling sleep mode...");
	setSleepEnabled(false);

	// get MPU hardware revision
	DEBUG_PRINTLN("Selecting user bank 16...");
	setMemoryBank(0x10, true, true);
	DEBUG_PRINTLN("Selecting memory byte 6...");
	setMemoryStartAddress(0x06);
	DEBUG_PRINTLN("Checking hardware revision...");
	uint8_t hwRevision = readMemoryByte();
	DEBUG_PRINT("Revision @ user[16][6] = ");
	DEBUG_PRINTLNF(hwRevision, HEX);
	DEBUG_PRINTLN("Resetting memory bank selection to 0...");
	setMemoryBank(0, false, false);

	// check OTP bank valid
	DEBUG_PRINTLN("Reading OTP bank valid flag...");
	uint8_t otpValid = getOTPBankValid();
	DEBUG_PRINT("OTP bank is ");
	DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));

	// get X/Y/Z gyro offsets
	DEBUG_PRINTLN("Reading gyro offset TC values...");
	int8_t xgOffsetTC = getXGyroOffsetTC();
	int8_t ygOffsetTC = getYGyroOffsetTC();
	int8_t zgOffsetTC = getZGyroOffsetTC();
	DEBUG_PRINT("X gyro offset = ");
	DEBUG_PRINTLNi(xgOffsetTC);
	DEBUG_PRINT("Y gyro offset = ");
	DEBUG_PRINTLNi(ygOffsetTC);
	DEBUG_PRINT("Z gyro offset = ");
	DEBUG_PRINTLNi(zgOffsetTC);

	// setup weird slave stuff (?)
	/*    DEBUG_PRINTLN("Setting slave 0 address to 0x7F...");
	    setSlaveAddress(0, 0x7F);
	    DEBUG_PRINTLN("Disabling I2C Master mode...");
	    setI2CMasterModeEnabled(false);
	    DEBUG_PRINTLN("Setting slave 0 address to 0x68 (self)...");
	    setSlaveAddress(0, 0x68);
	    DEBUG_PRINTLN("Resetting I2C Master control...");
	    resetI2CMaster();
			mpuDelayMs(20);*/




	// DEBUG_PRINTLN("Enabling interrupt latch, clear on any read, AUX bypass enabled");
	// WriteReg(devAddr, MPU9250_RA_INT_PIN_CFG, 0x32);

	// enable MPU AUX I2C bypass mode
	//DEBUG_PRINTLN("Enabling AUX I2C bypass mode...");
	//setI2CBypassEnabled(true);

	// DEBUG_PRINTLN("Setting magnetometer mode to power-down...");
	// //mag -> setMode(0);
	// WriteReg(0x0E, 0x0A, 0x00);

	// DEBUG_PRINTLN("Setting magnetometer mode to fuse access...");
	// //mag -> setMode(0x0F);
	// WriteReg(0x0E, 0x0A, 0x0F);

	// DEBUG_PRINTLN("Reading mag magnetometer factory calibration...");
	// int8_t asax, asay, asaz;
	// //mag -> getAdjustment(&asax, &asay, &asaz);
	// ReadBuf(0x0E, 0x10, 3, buffer);
	// asax = (int8_t)buffer[0];
	// asay = (int8_t)buffer[1];
	// asaz = (int8_t)buffer[2];
	// DEBUG_PRINT("Adjustment X/Y/Z = ");
	// DEBUG_PRINT(asax);
	// DEBUG_PRINT(" / ");
	// DEBUG_PRINT(asay);
	// DEBUG_PRINT(" / ");
	// DEBUG_PRINTLN(asaz);













	// load DMP code into memory banks
	DEBUG_PRINT("Writing DMP code to MPU memory banks (");
	DEBUG_PRINTi(MPU9250_DMP_CODE_SIZE);
	DEBUG_PRINTLN(" bytes)");
	if (writeProgMemoryBlock(dmpMemory, MPU9250_DMP_CODE_SIZE))
	{
		DEBUG_PRINTLN("Success! DMP code written and verified.");

		// write DMP configuration
		DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
		DEBUG_PRINTi(MPU9250_DMP_CONFIG_SIZE);
		DEBUG_PRINTLN(" bytes in config def)");
		if (writeProgDMPConfigurationSet(dmpConfig, MPU9250_DMP_CONFIG_SIZE))
		{
			DEBUG_PRINTLN("Success! DMP configuration written and verified.");

			DEBUG_PRINTLN("Setting clock source to Z Gyro...");
			setClockSource(MPU9250_CLOCK_PLL_ZGYRO);

			DEBUG_PRINTLN("Setting DMP and FIFO_OFLOW interrupts enabled...");
			setIntEnabled(0x12);

			DEBUG_PRINTLN("Setting sample rate to 200Hz...");
			setRate(4); // 1khz / (1 + 4) = 200 Hz

			DEBUG_PRINTLN("Setting external frame sync to TEMP_OUT_L[0]...");
			setExternalFrameSync(MPU9250_EXT_SYNC_TEMP_OUT_L);

			DEBUG_PRINTLN("Setting DLPF bandwidth to 42Hz...");
			setDLPFMode(MPU9250_DLPF_BW_42);

			DEBUG_PRINTLN("Setting gyro sensitivity to +/- 2000 deg/sec...");
			setFullScaleGyroRange(MPU9250_GYRO_FS_2000);

			DEBUG_PRINTLN("Setting DMP configuration bytes (function unknown)...");
			setDMPConfig1(0x03);
			setDMPConfig2(0x00);

			DEBUG_PRINTLN("Clearing OTP Bank flag...");
			setOTPBankValid(false);

			DEBUG_PRINTLN("Setting X/Y/Z gyro offset TCs to previous values...");
			setXGyroOffsetTC(xgOffsetTC);
			setYGyroOffsetTC(ygOffsetTC);
			setZGyroOffsetTC(zgOffsetTC);

			//DEBUG_PRINTLN("Setting X/Y/Z gyro user offsets to zero...");
			//setXGyroOffset(0);
			//setYGyroOffset(0);
			//setZGyroOffset(0);




			DEBUG_PRINTLN("Writing final memory update 1/19 (function unknown)...");
			uint8_t dmpUpdate[16], j;
			uint16_t pos = 0;
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Writing final memory update 2/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Resetting FIFO...");
			resetFIFO();

			DEBUG_PRINTLN("Reading FIFO count...");
			uint8_t fifoCount = getFIFOCount();

			DEBUG_PRINTLN("Current FIFO count=%d"), fifoCount;
			uint8_t fifoBuffer[128];
			//getFIFOBytes(fifoBuffer, fifoCount);

			DEBUG_PRINTLN("Writing final memory update 3/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Writing final memory update 4/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Disabling all standby flags...");
			writeByte(0x68, MPU9250_RA_PWR_MGMT_2, 0x00);

			DEBUG_PRINTLN("Setting accelerometer sensitivity to +/- 2g...");
			writeByte(0x68, MPU9250_RA_ACCEL_CONFIG, 0x00);

			DEBUG_PRINTLN("Setting motion detection threshold to 2...");
			setMotionDetectionThreshold(2);

			DEBUG_PRINTLN("Setting zero-motion detection threshold to 156...");
			setZeroMotionDetectionThreshold(156);

			DEBUG_PRINTLN("Setting motion detection duration to 80...");
			setMotionDetectionDuration(80);

			DEBUG_PRINTLN("Setting zero-motion detection duration to 0...");
			setZeroMotionDetectionDuration(0);

			DEBUG_PRINTLN("Setting AK8975 to single measurement mode...");


			// setup AK8975 (0x0E) as Slave 0 in read mode
			DEBUG_PRINTLN("Setting up AK8975 read slave 0...");
			writeByte(0x68, MPU9250_RA_I2C_SLV0_ADDR, 0x80 | MPU9250_MAG_ADDRESS);
			writeByte(0x68, MPU9250_RA_I2C_SLV0_REG,  0x01);
			setSlaveWordGroupOffset(0, true);
			setSlaveWordByteSwap(0, true);
			setSlaveDataLength(0, 9);
			setSlaveEnabled(0, true);
			// writeByte(0x68, MPU9250_RA_I2C_SLV0_CTRL, 0xDA);

			// // setup AK8975 (0x0E) as Slave 2 in write mode
			DEBUG_PRINTLN("Setting up AK8975 write slave 2...");
			// writeByte(0x68, MPU9250_RA_I2C_SLV2_ADDR, MPU9250_MAG_ADDRESS);
			// writeByte(0x68, MPU9250_RA_I2C_SLV2_REG,  MPU9250_MAG_CNTL);
			// writeByte(0x68, MPU9250_RA_I2C_SLV2_CTRL, 0x81);
			// writeByte(0x68, MPU9250_RA_I2C_SLV2_DO,   0x11);

			// setup I2C timing/delay control
			DEBUG_PRINTLN("Setting up slave access delay...");
			writeByte(0x68, MPU9250_RA_I2C_SLV4_CTRL, 0x18);
			writeByte(0x68, MPU9250_RA_I2C_MST_DELAY_CTRL, 0x05);

			// enable interrupts
			DEBUG_PRINTLN("Enabling default interrupt behavior/no bypass...");
			writeByte(0x68, MPU9250_RA_INT_PIN_CFG, 0x00);


			// enable I2C master mode and reset DMP/FIFO
			DEBUG_PRINTLN("Enabling I2C master mode...");
			writeByte(0x68, MPU9250_RA_USER_CTRL, 0x20);
			DEBUG_PRINTLN("Resetting FIFO...");
			writeByte(0x68, MPU9250_RA_USER_CTRL, 0x24);
			DEBUG_PRINTLN("Rewriting I2C master mode enabled because...I don't know");
			writeByte(0x68, MPU9250_RA_USER_CTRL, 0x20);
			DEBUG_PRINTLN("Enabling and resetting DMP/FIFO...");
			writeByte(0x68, MPU9250_RA_USER_CTRL, 0xE8);

			mpuWriteSlaveReg(MPU9250_MAG_ADDRESS, MPU9250_MAG_CNTL, 0x16);

			DEBUG_PRINTLN("Writing final memory update 5/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 6/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 7/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 8/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 9/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 10/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 11/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Reading final memory update 12/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
#ifdef DEBUG
			DEBUG_PRINT("Read bytes: ");
			for (j = 0; j < 4; j++)
			{
				DEBUG_PRINTLN("%x ", dmpUpdate[3 + j]);
			}
			DEBUG_PRINTLN("");
#endif

			DEBUG_PRINTLN("Writing final memory update 13/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 14/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 15/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 16/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
			DEBUG_PRINTLN("Writing final memory update 17/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Waiting for FIRO count >= 46...");
			while ((fifoCount = getFIFOCount()) < 46);
			DEBUG_PRINTLN("Reading FIFO...");
			getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
			DEBUG_PRINTLN("Reading interrupt status...");
			getIntStatus();

			DEBUG_PRINTLN("Writing final memory update 18/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Waiting for FIRO count >= 48...");
			while ((fifoCount = getFIFOCount()) < 48);
			DEBUG_PRINTLN("Reading FIFO...");
			getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
			DEBUG_PRINTLN("Reading interrupt status...");
			getIntStatus();
			DEBUG_PRINTLN("Waiting for FIRO count >= 48...");
			while ((fifoCount = getFIFOCount()) < 48);
			DEBUG_PRINTLN("Reading FIFO...");
			getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
			DEBUG_PRINTLN("Reading interrupt status...");
			getIntStatus();

			DEBUG_PRINTLN("Writing final memory update 19/19 (function unknown)...");
			for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
			writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);

			DEBUG_PRINTLN("Disabling DMP (you turn it on later)...");
			setDMPEnabled(false);

			DEBUG_PRINTLN("Setting up internal 48-byte (default) DMP packet buffer...");
			dmpPacketSize = 48;
			/*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
			    return 3; // TODO: proper error code for no memory
			}*/

			DEBUG_PRINTLN("Resetting FIFO and clearing INT status one last time...");
			resetFIFO();
			getIntStatus();
		}
		else
		{
			DEBUG_PRINTLN("ERROR! DMP configuration verification failed.");
			return 2; // configuration block loading failed
		}
	}
	else
	{
		DEBUG_PRINTLN("ERROR! DMP code verification failed.");
		return 1; // main binary block loading failed
	}
	return 0; // success
}

uint8_t MPU9250::dmpGetAccel(int16_t *data, const uint8_t* packet)
{
	// TODO: accommodate different arrangements of sent data (ONLY default supported now)
	if (packet == 0) packet = dmpPacketBuffer;
	data[0] = (packet[34] << 8) + packet[35];
	data[1] = (packet[38] << 8) + packet[39];
	data[2] = (packet[42] << 8) + packet[43];
	return 0;
}
uint8_t MPU9250::dmpGetGyro(int16_t *data, const uint8_t* packet)
{
	// TODO: accommodate different arrangements of sent data (ONLY default supported now)
	if (packet == 0) packet = dmpPacketBuffer;
	data[0] = (packet[16] << 8) + packet[17];
	data[1] = (packet[20] << 8) + packet[21];
	data[2] = (packet[24] << 8) + packet[25];
	return 0;
}
uint8_t MPU9250::dmpGetMag(int16_t *data, const uint8_t* packet)
{
	// TODO: accommodate different arrangements of sent data (ONLY default supported now)
	if (packet == 0) packet = dmpPacketBuffer;
	data[0] = (packet[28] << 8) + packet[29];
	data[1] = (packet[30] << 8) + packet[31];
	data[2] = (packet[32] << 8) + packet[33];
	return 0;
}
